/*
 * Image_process.c
 *
 *  Created on: 2017年7月17日
 *      Author: Case
 */

#include	"camera.h"
#include "r_cg_macrodriver.h"
#include "r_cg_sci.h"
#include "Image_process.h"
#include "math.h"
#include<stdlib.h>
#include<stdio.h>
/*
 * 寻中心黑点
 * */
void	seek_mid(uint8_t picture[OV7670_PIX_HEIGHT][OV7670_PIX_WIDTH])
{
	uint8_t		mid[OV7670_PIX_HEIGHT];
	uint8_t		count_l = 0;
	uint8_t		count_r = 0;
	uint16_t 	i,j;
	uint16_t 	edge_l = 0;
	uint16_t	edge_r = 0;
	for(i = 0;i < OV7670_PIX_HEIGHT;i++)
	{
		count_l = 0;
		count_r = 0;
		for(j = 1;j < OV7670_PIX_WIDTH;j++)
		{
			if((picture[i][j-1] == 0 && picture[i][j] != 0 && picture[i][j+1] == 0xff) || (picture[i][j-1] == 0&&picture[i][j] == 0xff)) //寻找左边界
			{
				if(count_l == 1)
				{
					continue;
				}
				edge_l = j/2;
				count_l ++;
			}
			else if((picture[i][j-1] == 0xff && picture[i][j] != 0 && picture[i][j+1] == 0)||(picture[i][j-1] == 0xff && picture[i][j] == 0))//寻找右边界
			{
				if(count_r == 1)
				{
					continue;
				}
				edge_r = j/2;
				count_r++;
			}
		}
		mid[i] = edge_l + edge_r; //确定中心点
	}
//	R_SCI1_Serial_Send_Data(mid,20);
	method(mid);
}

/*最小二乘法对取得的中心点进行直线拟合
  并求出拟合后的直线的斜率            */

void	method(uint8_t mid[OV7670_PIX_HEIGHT])			//计算斜率
{
	int i,j;
	double x_avg = 0,y_avg = 0,xy_avg = 0,xx_avg = 0,yy_avg = 0;
	double	sum_x = 0,sum_y = 0,k = 0,b;
	double	sum_xy = 0,sum_xx = 0,sum_yy = 0;
	for(i = OV7670_PIX_HEIGHT - 1; i >= 0; i -= 10)
	{
		for(j = 0;j < NUM;j++)
		{
			sum_x += mid[i-j];	//横坐标之和
			sum_y += i-j;		    //纵坐标之和
			sum_xx += mid[i-j]*mid[i-j]; //x^2的和
			sum_yy += (i-j)*(i-j);			  //y^2的和
			sum_xy += mid[i-j]*(i-j);		  //x*y的和
		}

		x_avg = sum_x/OV7670_PIX_HEIGHT;	//计算横坐标X的平均值
		y_avg = sum_y/OV7670_PIX_HEIGHT; //计算纵坐标Y的平均值
		xx_avg = sum_xx/OV7670_PIX_HEIGHT; //x^2的平均值
		xy_avg = sum_xy/OV7670_PIX_HEIGHT; //横纵坐标xy和的平均值
		yy_avg = sum_yy/OV7670_PIX_HEIGHT;

	  	if((x_avg*y_avg-xy_avg)!=0)
		{

			  	k=(y_avg*y_avg - yy_avg)/(x_avg*y_avg-xy_avg);//计算拟合后的直线的斜率
		        b=(x_avg*y_avg*y_avg-y_avg*xy_avg)/(x_avg*y_avg-xy_avg);//计算拟合后的直线的纵截距
		        x_avg=y_avg=xx_avg=xy_avg=yy_avg=0; //计算一次之计算斜率后清零相关的数值
		     	sum_x=sum_y=sum_xx=sum_xy=sum_yy=0;//计算一次之后清零相关的数值
		     	correct_error(k,b);//每计算一次斜率和截距，就进行误差判断
			}
			else
			{
				break;
			}
		}
}

void correct_error(float k,float b)
{
	float distance_error=0;//飞机的中轴线偏离路径中心线的距离误差
	float angle_error=0,rad_error;//飞机中轴线与路径中心线所成的夹角误差
	float a1,b1,c1;
	int	row = OV7670_PIX_WIDTH*8;
	int	col = OV7670_PIX_HEIGHT;
	a1=k;
	b1=-1;
	c1=b;

	distance_error=(row-1)/2+(b1*(col-1))/(2*a1)+c1/a1;//计算偏离中心线的误差

	if(a1*b1>0)//计算方向偏差
	{
		rad_error=PI/2-fabs(atan(a1/b1));
		angle_error=(180*rad_error)/PI;
	}
	else if(a1*b1<0)
	{
		rad_error=fabs(atan(a1/b1))-PI/2;
		angle_error=(180*rad_error)/PI;
	}
	R_SCI1_Serial_Send_Data(&distance_error,1);
	R_SCI1_Serial_Send_Data(&angle_error,1);
}

/*********
 *
 * @param picture
 *
 * 			X1				X2
 * 	__________________
 * 	|		|				|		|
 * 	|___ |_______	|___ |	Y1
 * 	|		|				|		|
 * 	|		|				|		|
 * 	|		|				|		|
 * 	|		|				|		|
 * 	|___ |_______  |___ |	Y2
 * 	|		|				|       |
 * 	|___ |_______  |___ |
 */

void	Heading_decision(uint8_t picture[OV7670_PIX_HEIGHT][OV7670_PIX_WIDTH])
{
	int	seek_x,seek_y;
	int	i,j;

	char	HDY1[OV7670_PIX_WIDTH];		//储存Y1线上所有的黑点
	char	HDY2[OV7670_PIX_WIDTH];		//储存Y2线上所有的黑点
	char	HDX1[OV7670_PIX_HEIGHT];	//储存X1线上所有的黑点
	char	HDX2[OV7670_PIX_HEIGHT];	//储存X2线上所有的黑点

	char	Y1_L[10] = {0};			//Y1线左边界
	char	Y2_L[10] = {0};			//Y2线左边界
	char	X1_L[10] = {0};			//X1线左边界
	char	X2_L[10] = {0};			//X2线左边界

	char	Y1_R[10] = {0};			//Y1线右边界
	char	Y2_R[10] = {0};			//Y2线右边界
	char	X1_R[10] = {0};			//X1线右边界
	char	X2_R[10] = {0};			//X2线右边界
	for(seek_x = 0;seek_x < OV7670_PIX_WIDTH;seek_x++)		//记录Y1线上的所有黑点
	{
		if(picture[HEADING_DECISION_Y1][seek_x] == 0xff)
		{
			HDY1[seek_x] = 1;
		}
		else
		{
			HDY1[seek_x] = 0;
		}
	}

	for(seek_x = 0;seek_x < OV7670_PIX_WIDTH;seek_x++)		//记录Y2线上的所有黑点
	{
		if(picture[HEADING_DECISION_Y2][seek_x] == 0xff)
		{
			HDY2[seek_x] = 1;
		}
		else
		{
			HDY2[seek_x] = 0;
		}
	}

	for(seek_y = 0;seek_y < OV7670_PIX_HEIGHT;seek_y++)		//记录X1线上的所有黑点
	{
		if(picture[seek_y][HEADING_DECISION_X1] == 0xff)
		{
			HDX1[seek_y] = 1;
		}
		else
		{
			HDX1[seek_y] = 0;
		}
	}

	for(seek_y = 0;seek_y < OV7670_PIX_HEIGHT;seek_y++)		//记录X2线上的所有黑点
	{
		if(picture[seek_y][HEADING_DECISION_X2] == 0xff)
		{
			HDX2[seek_y] = 1;
		}
		else
		{
			HDX2[seek_y] = 0;
		}
	}

	Four_Seek_edge(HDY1,OV7670_PIX_WIDTH,Y1_L,Y1_R);			//寻找Y1上的左右边界
	Four_Seek_edge(HDY2,OV7670_PIX_WIDTH,Y2_L,Y2_R);			//寻找Y2上的左右边界
	Four_Seek_edge(HDX1,OV7670_PIX_HEIGHT,X1_L,X1_R);		//寻找X1上的左右边界
	Four_Seek_edge(HDX2,OV7670_PIX_HEIGHT,X2_L,X2_R);		//寻找X2上的左右边界

	if(Y1_L[0] > Y2_L[0])					//************     /					*/
	{
		if(Y1_R[0] > Y2_R[0])				//***********    //					*/
		{

		}
		else if(Y1_R[0] < Y2_R[0])		//***********   / \ 			        */
		{

		}
		else											//***********		/|					*/
		{

		}
	}
	else if(Y1_L[0] < Y2_L[0])			//************		\				 */
	{
		if(Y1_R[0] < Y2_R[0])				//************		\\				 */
		{

		}
		else if(Y1_R[0] > Y2_R[0])		//************		\ /				 */
		{

		}
		else											//************		\|				 */
		{

		}
	}
	else
	{

	}


}

void	Four_Seek_edge(char HD[],int	len,char XY_L[10],char XY_R[10])		//寻找左右边界函数
{
	int	i=0,j=0;
	for(i = 0;i < len;i++)				//寻找到线上线段的左边界
	{
		if(HD[i-1] == 0&&HD[i] == 1&&HD[i+1]==1)  //出现0->11
		{
			if(HD[i-2] == 0 &&HD[i+2] == 1)	//接着判断是否为00->111
			{
				XY_L[j] = i;
				j++;
			}
		}
	}

	j = 0;

	for(i = 0;i<len;i++)					//寻找到线上线段的左边界
	{
		if(HD[i-1] == 1&&HD[i] == 0&&HD[i+1]==0)  //出现1->00
		{
			if(HD[i-2] == 0 &&HD[i+2] == 1)	//接着判断是否为11->00
			{
				XY_R[j] = i;
				j++;
			}
		}
	}


}
